#ifndef __SOLVE3D_H
#define __SOLVE3D_H
//#include "headers.h"
#include "keyframe.h"
#include "Marker.h"

using markerAR::Keyframe;
namespace Solve3D
{

	 Matx33d getRotation(const double angle[3]);
     void setKMatrix(Mat &KMatrix,double imageWidth, double imageHeight, double focus);
     void solveSRT(Mat &pts3d1, Mat &pts3d2, double &s, Mat &R, Mat &t);
     void transformSRT(const vector<Point3f> &pts3d1,vector<Point3f> &pts3d2,double s,Mat R,Mat t);
     void Calibrate_Points(Point2f pp,double focal,vector<Point2f> &origin,vector<Point2f> &out);
//     bool Solve5Point(vector<Point2f> &pts1, vector<Point2f> &pts2, int num_pts, Mat &E, Mat &P, std::vector<int> &ret_inliers);
//     Mat setRotationMatrix(const CMRotationMatrix &rotation);//init the R
    Eigen::Matrix3d setRotationMatrix(double yaw,double pitch,double roll);
    void get3DPoints(const cv::Matx33d& KMatrix,const cv::Matx33d &rotationMatrix, const cv::Size &imgSize, Mat &pts3D,Matx31d &t);
     void get3DPoints(const cv::Mat& KMatrix,const cv::Mat &rotationMatrix, const cv::Size &imgSize, Mat &pts3D,Mat &t);
     void sba(Mat KMatrix, vector<Keyframe> &image, const vector<markerAR::Marker> &pts3Ds,bool flag = true);
	 void get3DPoints(Matx33d KMatrix, Matx33d rotationMatrix, vector<KeyPoint> &pts2D, vector<Point3f> &pts3D, Matx31d &TVector);
    void get3DPoints(Matx33d KMatrix, Matx33d rotationMatrix, vector<vector<Feature*>> &pts2Ds,Matx31d &TVector,float scale,int levelNum);
    void get3DPoints(Mat KMatrix, const Mat rotationMatrix, const Mat TVector, const vector<KeyPoint> &pts2D, vector<Point3f> &pts3D);
     Mat getNextInvertMatrix(Mat KMatrix,Mat R,Mat t);
     bool getCameraRT(const Matx33d &KMatrix,const vector<Point3f> &P3D,const vector<Point2f> &P2D,Matx33d &R,Matx31d &t,bool isTracked);
     void get2DPoint(Mat KMatrix,Mat R,Mat t,vector<Point3f> &points3DVector,vector<Point2f> &points2DVector,Mat &drawMatch);
     Matx44d getP(const Matx33d &R,const Matx31d &t);
     void printPlane(Mat KMatrix,Mat R,Mat t,cv::Mat &drawPlane, double lowX, double lowY, double highX, double highY);
     void getHighAndLow(vector<Point3f> &points3DVector,double &lowX, double &lowY, double &highX, double &highY);
     void Slerp(const double &w1, const Scalar &q1, const Scalar &q2, Scalar &output);
     void ToRotationMatrix(const Scalar q, Matx33d &R);
     void FromRotationMatrix(const Matx33d &R, Scalar &q);
     void Tlerp(const double &w1, const Matx31d &origin, const Matx31d &optimised, Matx31d &output);
	 void Homography_Decomposition(const Matx33d &Kmatrix, const Matx33d&H, Matx33d &R, Matx31d &t);
     bool getCameraRt(const Mat &KMatrix,const Mat &refImg,const vector<Point2f> &refPoints,const Mat& currentImg,const vector<Point2f> &currentPoints,Mat &R,Mat &t);
     double computeReprojectedError(const vector<Point3f> &objectPoints,const vector<Point2f> &imagePoints,const Matx33d&KMatrix,const Matx33d&R,const Matx31d&t);
    Eigen::Matrix3d HfromPose(const Eigen::Matrix3d&K,const Pose&C1,const Pose &C2,const Eigen::Vector3d&N,const double&D);
     void get3DPoints(Matx33d KMatrix, Matx33d rotationMatrix, vector<vector<KeyPoint>> &pts2Ds, vector<vector<Point3f>> &pts3Ds, Matx31d &TVector,float scale,int levelNum);
    void get3DPoints(const Eigen::Matrix3d &KMatrix, const Eigen::Matrix3d& rotationMatrix, vector<vector<Feature*>> &pts2Ds,Eigen::Vector3d &TVector,float scale,int levelNum);
    void get3DPoints(const Eigen::Matrix3d& KMatrix,const Eigen::Matrix3d &rotationMatrix,vector<KeyPoint> &pts2D,vector<Point3f> &pts3D,Eigen::Vector3d &TVector);
    Eigen::Matrix4d getP(const Eigen::Matrix3d &R,const Eigen::Vector3d &t);
    Eigen::Matrix4d PosetoGL(const Eigen::Matrix3d &R,const Eigen::Vector3d &t);
    
}




#endif